import ctypes
from ctypes import *
import time
import smbus
import sys; 
sys.path.append("/home/pi/.local/lib/python3.5/site-packages/") 
import VL53L0X
import math
import Adafruit_ADS1x15

ADC_TO_VOLT = (4.096/2048)*2
adc = Adafruit_ADS1x15.ADS1015(address=0x48, busnum=3)


# argument
location_x = 0
location_y = 0
angle = 0

err_angle = 0
last_err_angle = 0
last_last_err_angle = 0

# control F4
adder = CDLL('/home/pi/Pi-puck/e-puck2/pymain.so')
wantime = 2
sttime = 0.05
init_speed = 900

# VL53L0X test
# Create a VL53L0X object
tof = VL53L0X.VL53L0X(i2c_bus=4, i2c_address=0x29)
tof.open()
tof.start_ranging(VL53L0X.Vl53l0xAccuracyMode.BETTER)
timing = tof.get_timing()
distance = tof.get_distance()

num = 0
location_x = 0
location_y = 0

goal_x = 0
goal_y = 0

count = 0
battery_level = 0

# get data from F1 and control RGBS of F1
def set_get_F1(num):
    # init the I2C_CHANNEL
    I2C_CHANNEL = 3
    FT903_I2C_ADDR = 0x14
    bus = smbus.SMBus(I2C_CHANNEL)
    revdata = [160, 1, 2, 3, 4, 5, 6, 166, 4, 5, 6]
    startflag = 0
    revnum = 0
    timecount = 0
    cled = 0

    loacation_x = 0
    loacation_y = 0
    angle = 0

    global goal_x
    global goal_y
    global battery_level
    global count

    # get the location
    for ii in range(0, 42):  # max read 24 bytes
        loop = bus.read_byte(FT903_I2C_ADDR)
        # print("iiii", ii, "---", loop)
        time.sleep(0.0001)

        if loop == 160:  # rev 0xA0
            startflag = 1
            revnum = 0
            revdata.clear()
        if startflag:
            revdata.append(loop)
            revnum += 1
        if revnum >= 14:  # and revdata[1]>50:
            loacation_x = revdata[1] * 255 + revdata[2]
            loacation_y = revdata[3] * 255 + revdata[4]
            angle = (-1) ** (revdata[5]) * revdata[6]

            goal_x = revdata[7] * 255 + revdata[8]
            goal_y = revdata[9] * 255 + revdata[10]

            print("rev data:", revdata, "x:%d" % loacation_x, "y:%d" % loacation_y, "angle:%d" % angle)
            print("rev data:", revdata, "goal_x:%d" % goal_x, "goal_y:%d" % goal_y)
            break

    
    # control rgb LED,  low the times
    count = count + 1
    if count > 38:
       count = 0
       
    if count == 0:  # control the rgb at 0 state
        print("dis00000000000000:",adder.get_the_block_sensor(0))
        adder.control_4RGBLEDS(0, 100,0,100, 100,0,100, 100,0,100, 100,0,100)
        adder.control_4RGBLEDS(1, 0,0,100, 100,0,100, 0,0,100, 100,0,100)
        
    if count == 20:  # control the rgb at 1 state
        print("dis777777777777:",adder.get_the_block_sensor(7))
        adder.control_4RGBLEDS(0, 0,80,10, 10,0,100, 100,0,10, 10,80,100)
        adder.control_4RGBLEDS(1, 0,80,10, 10,0,100, 100,0,10, 10,80,100)
        
    if count == 39:  # control the rgb at 1 state
        bat_epuck = adc.read_adc(0, gain=1) # Range is +/- 4.096V.
        bat_ext = adc.read_adc(1, gain=1) # Range is +/- 4.096V.
        print("e-puck: " + str(bat_epuck*ADC_TO_VOLT) + "V (" + str(bat_epuck) + "), ext: " + str(bat_ext*ADC_TO_VOLT) + "V (" + str(bat_ext) + ")")
        
        battery_level = int(((bat_epuck*ADC_TO_VOLT - 3.2)/(4.0-3.2))*100) # get the pecentage of battery energy
        if battery_level>100:
            battery_level=100
        elif battery_level<0:
            battery_level = 0
        print("battery_level,,,,,,,,,,,", battery_level)
        
        bus.write_byte(FT903_I2C_ADDR, 0xDA)
        time.sleep(0.01)
        bus.write_byte(FT903_I2C_ADDR, 0x01)
        time.sleep(0.01)
        bus.write_byte(FT903_I2C_ADDR, 0)
        time.sleep(0.01)
        bus.write_byte(FT903_I2C_ADDR, 0)
        time.sleep(0.01)
        bus.write_byte(FT903_I2C_ADDR, 0)
        time.sleep(0.01)
        bus.write_byte(FT903_I2C_ADDR, 0)
        time.sleep(0.01)
        bus.write_byte(FT903_I2C_ADDR, battery_level)
    
    return loacation_x, loacation_y, angle


# control the speed and the turningspeed of car
def contorl_car_speed(speed, turningspeed):
    # speed: the speed of the car,>0 front,<0 -back
    # turning: direction and the level of the car  >= 0: turn right  < 0:  # turn left
    leftspeed = speed + turningspeed
    rightspeed = speed - turningspeed

    max = 1000
    if leftspeed > max:
        leftspeed = max
    if leftspeed < -max:
        leftspeed = -max
    if rightspeed > max:
        rightspeed = max
    if rightspeed < -max:
        rightspeed = -max

    adder.control_motor_speed(int(leftspeed), int(rightspeed))


# get the goal angle
def get_car_goalangle(detination, location):
    goalangle = 0

    dx = detination[0] - location[0]
    dy = detination[1] - location[1]

    # get the goal angle
    if dy > 0 and dx > 0:
        arcangle = math.atan(dy / dx)
        goalangle = int((arcangle / 3.1415926) * 180)
        goalangle = 90 - goalangle
    elif dy < 0 and dx < 0:
        arcangle = math.atan(dy / dx)
        goalangle = int((arcangle / 3.1415926) * 180)
        goalangle = - goalangle - 90
    elif dy > 0 and dx < 0:
        arcangle = math.atan(dy / dx)
        goalangle = int((arcangle / 3.1415926) * 180)
        goalangle = -90 - goalangle
    elif dy < 0 and dx > 0:
        arcangle = math.atan(dy / dx)
        goalangle = int((arcangle / 3.1415926) * 180)
        goalangle = 90 - goalangle
    elif dy > 0 and dx == 0:
        goalangle = 0
    elif dy < 0 and dx == 0:
        goalangle = 180
    elif dx > 0 and dy == 0:
        goalangle = 90
    elif dx < 0 and dy == 0:
        goalangle = -90

    return goalangle


# get the detination,location, speed,then output the speed and the turning speed,then control motor
def get_car_todestination(goalangle, nowangle, speed):
    P = 5.5
    D = 4.5
    I = 0.1

    global err_angle
    global last_err_angle
    global last_last_err_angle
    global init_speed
    global num, location_x, location_y
    finalspeed = 0

    # handle the angle
    if 90 <= nowangle <= 180 and -180 <= goalangle <= -90:
        goalangle = goalangle + 360
    if 90 <= goalangle <= 180 and -180 <= nowangle <= -90:
        nowangle = nowangle + 360

    # get the turningspeed
    last_last_err_angle = last_err_angle
    last_err_angle = err_angle
    err_angle = goalangle - nowangle
    I_all = err_angle + last_err_angle + last_last_err_angle
    D_all = err_angle - 2 * last_err_angle + last_last_err_angle
    turningspeed = P * err_angle - D * D_all + I * I_all
    turningspeed = int(turningspeed)
    turningspeed = -turningspeed

    print("goalangle----------------goalangle", goalangle)
    print("turningspeed", turningspeed)


    # control the car
    if not -30 < goalangle - nowangle < 30:
        finalspeed = 0
        print("blocking----------------------------------cccccccccccccccccccccc")
    else:
        finalspeed = speed
    contorl_car_speed(finalspeed, turningspeed)


if __name__ == "__main__":
    goal_list = [[150, 300]]

    speed = init_speed

    # get now location and angle
    location_x, location_y, angle = set_get_F1(0)

    # get to destination, speed, location, angle
    goalangle = get_car_goalangle(goal_list[0], (location_x, location_y))  # get goal goalangle
    get_car_todestination(goalangle, angle, 900)

    while not -8 < goalangle - angle < 8:
        goalangle = get_car_goalangle(goal_list[0], (location_x, location_y))  # get goal goalangle
        location_x, location_y, angle = set_get_F1(0)
        get_car_todestination(goalangle, angle, 0)
        time.sleep(0.05)



    while True:
        num = 0
        goal_list[0] = [goal_x, goal_y]

        print("     ")
        print("goal---------------goal:", goal_list[0])

        # get now location and angle
        location_x, location_y, angle = set_get_F1(0)

        err_x = goal_list[0][0] - location_x
        err_y = goal_list[0][1] - location_y
        
        speed = init_speed      
        
        # get to destination, speed, location, angle
        goalangle = get_car_goalangle(goal_list[0], (location_x, location_y))  # get goal goalangle
        
        if -10 < err_x < 10 and -10 < err_y < 10:
            goalangle = angle
            res_int = adder.control_motor_speed(0, 0)
            speed = 0
            print("have arrive a goal++++++++++++++++++++++++++++++++++++have arrive a goal")
        else:
            get_car_todestination(goalangle, angle, speed)

        # get the distance front
        distance = tof.get_distance()
        print("distance: %d mm, %d cm" % (distance, (distance / 10)))

      
        # low speed or stop
        if 80 <= distance < 120:
            while 80 <= distance < 120:
                distance = tof.get_distance()
                time.sleep(0.05)
                speed = int(init_speed/2)
                location_x, location_y, angle = set_get_F1(0)
                goalangle = get_car_goalangle(goal_list[0], (location_x, location_y))  # get goal goalangle
                get_car_todestination(goalangle, angle, speed)
                print("blocking----------------------------------blocking")
        elif distance < 80:
            while distance < 80:
                distance = tof.get_distance()
                time.sleep(0.05)
                print("blocking----------------------------------blocking")
                contorl_car_speed(0, 0)
        






